#include "core/verlet.hpp"
#include "yocto/code/unroll.hpp"

namespace FastMD
{
	Verlet:: Verlet(  const Real time_step ) throw() :
	Step( time_step ),
	half_dt( time_step * __HALF )
	{
		
	}
	
	Verlet:: ~Verlet() throw()
	{
		
	}
	
	
	size_t Verlet:: vectors() const throw() { return 3; }
	
	void Verlet:: predict( Frame &frame ) throw()
	{
		assert( frame.vectors >= vectors() );
		V3D       *r    = &frame[ Frame::POSITIONS     ][0];
		V3D       *v    = &frame[ Frame::VELOCITIES    ][0];
		const V3D *a    = &frame[ Frame::ACCELERATIONS ][0];
		PBC_Proc   pbc  = frame.pbc_r; assert( pbc != NULL );
		const V3D &box  = frame.box;
		const V3D &ibox = frame.ibox;
		
#define VPRED(i)              \
v[i].muladd( half_dt, a[i]  );\
r[i].muladd( dt, v[i]  );     \
pbc( r[i], box, ibox)
		
		YOCTO_LOOP_FUNC_(frame.particles,VPRED,0);
		
	}
	
	void Verlet:: correct( Frame &frame, const bool isothermal ) throw()
	{
		assert( frame.vectors >= vectors() );
		V3D       *v = &frame[ Frame::VELOCITIES    ][0];
		const V3D *a = &frame[ Frame::ACCELERATIONS ][0];
		Real       K = 0;
#define VCORR(i)               \
v[i].muladd( half_dt, a[i] );  \
K += v[i].data * v[i].normsq()
		YOCTO_LOOP_FUNC_(frame.particles,VCORR,0);
		K *= __HALF;
		if( isothermal )
		{
			frame.thermal_update_K( K );
		}
		else
			frame.K = K;
	}
	
	
}
